#include "WPILib.h"
#include "Constants.h"
#include "DriveTrain.h"

DriveTrain::DriveTrain(InputHandler *input) {
	m_input = input;

	m_frontRight = new Jaguar(FRONT_RIGHT);
	m_frontLeft = new Jaguar(FRONT_LEFT);
	m_backRight = new Jaguar(BACK_RIGHT);
	m_backLeft = new Jaguar(BACK_LEFT);
	m_frontSteer = new Victor(FRONT_STEER);
	m_backSteer = new Victor(BACK_STEER);
}

void DriveTrain::WheelDrive() {
	Tank(m_input->InputHandler::GetLeftJoy(), m_input->InputHandler::GetRightJoy());
	
	/*
	 * Second
	float wheel = m_input->GetWheelValue();
	//printf("WHEEL %f", wheel);
	if (-.2 <= wheel && wheel <= .2) {
		CrabDrive(0, m_input->GetDriveSpeed());
	} else {
		float back_angle;
		if (wheel >= 0) {
			back_angle = 1.25 * 5 * CRAB_ANGLE * (-(wheel - V_THRESHOLD));// / ((CRAB_THRESHOLD+V_THRESHOLD)/4));
		} else {
			back_angle = (9/8) * 5 * CRAB_ANGLE * (-(wheel + V_THRESHOLD));// / ((CRAB_THRESHOLD+V_THRESHOLD)/4));
		}
		VDrive(0, back_angle, m_input->GetDriveSpeed());
	}*/
	/*
	 * Original
	} else if (-CRAB_THRESHOLD <= wheel && wheel <= CRAB_THRESHOLD) {
		float target_angle = (CRAB_ANGLE * (wheel/CRAB_THRESHOLD));
		CrabDrive(target_angle, m_input->GetDriveSpeed());
	} else {
		float front_angle, back_angle;
		if (wheel >= 0) {
			//front_angle = CRAB_ANGLE;
			back_angle = 1.25 * 5 * CRAB_ANGLE * (-(wheel - ANGDIF));// / ((CRAB_THRESHOLD+V_THRESHOLD)/4));
		} else {
			//front_angle = -CRAB_ANGLE;
			back_angle = (9/8) * 5 * CRAB_ANGLE * (-(wheel + ANGDIF));// / ((CRAB_THRESHOLD+V_THRESHOLD)/4));
		}
		printf("BackAngle %f\n", back_angle);
		//VDrive(front_angle, back_angle, m_input->GetDriveSpeed());
		VDrive(0, back_angle, m_input->GetDriveSpeed());
	}*/
}

void DriveTrain::CrabDrive(float target_angle, float speed) {
	//printf(" CRABDRIVE: %f", target_angle);
	SetSpeed(speed);
	SteerFront(target_angle);
	SteerBack(target_angle);
}

void DriveTrain::VDrive(float front_angle, float back_angle, float speed) {
	//printf(" VDRIVE: %f %f", front_angle, back_angle);
	float dif  = front_angle - back_angle;
	if (dif < 0) dif = -dif;
	dif /= 90; dif /= DIFF_CONST;
	SetSpeed(speed, dif);
	SteerFront(front_angle);
	SteerBack(back_angle);
}

void DriveTrain::Tank(float right, float left) {
	m_frontRight->Set(left);
	m_frontLeft->Set(right);
	m_backRight->Set(left);
	m_backLeft->Set(right);
}

void DriveTrain::SetSpeed(float speed, float dif) {
	//printf("speed=%f\n", speed);
	if (dif >= 0){
		m_frontRight->Set(speed);
		m_frontLeft->Set(speed * (1 - dif));
		m_backRight->Set(speed);
		m_backLeft->Set(speed * (1 - dif));
		
	} else {
		m_frontRight->Set(speed * (1 + dif));
		m_frontLeft->Set(speed);
		m_backRight->Set(speed * (1 + dif));
		m_backLeft->Set(speed);
	}
}

void DriveTrain::SteerFront(float angle) {
	float out = GetOut(m_input->GetFrontAngle(), angle / (float)CRAB_ANGLE);
	//printf(" Front %f", out);
	if (angle < -1 && out < 0) out = 0;
	else if (angle > 1 && out > 0) out = 0;
	m_frontSteer->Set(out);
}

void DriveTrain::SteerBack(float angle) {
	float out = GetOut(m_input->GetBackAngle(), angle / (float)CRAB_ANGLE);
	//printf("Back %f", out);
	if (angle < -1 && out < 0) out = 0;
	else if (angle > 1 && out > 0) out = 0;
	m_backSteer->Set(out);
}
	
void DriveTrain::TestDrive() {
	/*float wheel = m_input->GetDriveSpeed(), angle = m_input->GetBackAngle(); //m_input->GetWheelValue();
	//SteerFront(wheel * CRAB_ANGLE);
	if (angle < -1 && wheel > 0) wheel = 0;
	else if (angle > 1 && wheel < 0) wheel = 0;
	m_backSteer->Set(wheel);
	printf("%f\n", m_input->GetBackAngle());*/
	
	float wheel = m_input->GetWheelValue();
	SteerFront(-CRAB_ANGLE * wheel);
	SetSpeed(m_input->GetDriveSpeed(), -(wheel / DIFF_CONST));
}

float GetOut(float real, float target) {
	float dif = target - real;
	int mult = -1;
	if (dif < 0) {dif = -dif; mult = 1;}
	//printf(" GetOut %f %f %f\n", real, target, dif);
	
	if (dif > .1 && dif < .3) return .1 * mult;
	else if (dif >= .3 && dif < .6) return .15 * mult;
	else if (dif >= .6) return .25 * mult;
	else return 0;
}
